package sdp;

import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;
import java.lang.reflect.Array;
import java.nio.ByteBuffer;
import java.nio.IntBuffer;

import lejos.pc.comm.NXTComm;
import lejos.pc.comm.NXTCommException;
import lejos.pc.comm.NXTCommFactory;
import lejos.pc.comm.NXTInfo;

public class Communication {
	/*
	public Communication(NXTComm nxtComm, NXTInfo info) {
		this.nxtComm = nxtComm;
		this.info = info;
	}
	*/
	
	private boolean useUSB = true;
	private boolean deviceFound = false;
	private boolean connected = false;
	
	private boolean dummyMode = false;
	
	private int [] tachos = new int [3];
	
	public void connect() throws IOException {
		
		if (this.dummyMode) {
			return;
		}
		
		NXTComm nxtComm = null;
		NXTInfo[] info  = null;
		
		try {
			if (this.useUSB) {
				//System.out.println("Communication: Using USB.");
				nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.USB);
				info = nxtComm.search(null, NXTCommFactory.USB);
			}
			else {
				///////////////////
			}
		
			try {
				nxtComm.open(info[0]);
				this.deviceFound = true;
			}
			catch (ArrayIndexOutOfBoundsException e) {
				
			}
		
		} catch (NXTCommException e1) {
			e1.printStackTrace();
			System.err.println("Communication: Cannot establish connection.");
		}
		
		if (this.deviceFound) {
			os = nxtComm.getOutputStream();
			is = nxtComm.getInputStream();
			
			System.out.println("Communication: Connection established.");
			
			this.connected = true;
		}
		else {
			//System.err.println("Communication: No device found.");
		}

	}
	
	public void disconnect() throws IOException {
		
		try {
			is.close();
			os.close();
		} catch (IOException e) {
			e.printStackTrace();
		}
		
	}
	
	public boolean isConnected() {
		if (this.dummyMode) {
			return true;
		}
		return this.connected;
	}
	
	private void sendDouble (double d) {
		byte[] bytes = ByteBuffer.allocate(8).putDouble(d).array();
		this.sendBytes(bytes);
	}
	
	private void sendInt (int i) {
		byte[] bytes = ByteBuffer.allocate(4).putInt(i).array();
		this.sendBytes(bytes);
	}
	
	private void sendBytes (byte[] b) {
		
		if (this.dummyMode) {
			return;
		}
		
		try {
			os.write(b);
			os.flush();
		} catch (IOException ex) {
			System.err.println("Communication: Error sending bytes to robot.");
			System.err.println(ex.toString());
		}
	}
	
//	private void sendCommand (int opcode) {
//		try {
//			byte[] bytes = ByteBuffer.allocate(4).putInt(opcode).array();
//			os.write(bytes);
//			os.flush();
//		} catch (IOException ex) {
//			System.out.println("Error sending command to robot");
//			System.out.println(ex.toString());
//		}
//	}
	
	public void update( double[] newMotorSpeeds ) {
		
		int [] motorSpeeds = new int[3];
		
		motorSpeeds[0] = (int)(newMotorSpeeds[0] * 180 / Math.PI);
		motorSpeeds[1] = (int)(newMotorSpeeds[1] * 180 / Math.PI);
		motorSpeeds[2] = (int)(newMotorSpeeds[2] * 180 / Math.PI);
		
		//System.out.println("SENDING SPEEDS: " + motorSpeeds[0] + " " + motorSpeeds[1] + " " + motorSpeeds[2]);
		
		ByteBuffer byteBuffer = ByteBuffer.allocate(newMotorSpeeds.length * 4);
		IntBuffer intBuffer = byteBuffer.asIntBuffer();
		intBuffer.put(motorSpeeds);
		
		this.sendBytes(byteBuffer.array());
		
		// Now that we sent the data - read the data from the brick
		
//		byte [] bytes = new byte [3*4]; // We are about to read 3 integers
//		
//		try {
//			is.read(bytes);
//		} catch (IOException e) {
//			// TODO Auto-generated catch block
//			e.printStackTrace();
//		}
		
//		int [] encoders = new int[3];
//		encoders = this.read3Bytes(bytes);
//		
//		this.tachos[0] += encoders[0];
//		this.tachos[1] += encoders[1];
//		this.tachos[2] += encoders[2];
		
		//System.out.println(this.tachos[0] + " " + this.tachos[1] + " " + this.tachos[2]);
		
	}
	
	private int [] read3Bytes(byte[] b) {
		int values [] = new int[3];
		for (int i = 0 ; i < 3 ; i++) {
			int value = 0;
			for (int j = 0 ; j < 4 ; j++) {
				int shift = (4 - 1 - j) * 8;
				value += (b[j+(i*4)] & 0x000000FF) << shift;
			}
			values[i] = value;
		}
		
		return values;
	}
	
	private OutputStream os;
	private InputStream is;
	NXTComm nxtComm;
	NXTInfo info;

	public double[] getEncoderValues() {
		double[] encoders =  { (double)this.tachos[0], (double)this.tachos[1], (double)this.tachos[2] };
		return encoders;
	}
}
